#ifndef TRAJECTORY_H
#define TRAJECTORY_H
#include "cdecl.h"
#include "cspline.h"

/**
 * This module provides trajectory abstraction. A trajectory is a 
 * function f:[0,1]->R^3. The module provides functions to find different
 * poperties from a trajectory.
 */

enum trajectory_type {
	TRAJ_LINE, /* linear */
	TRAJ_BEZIER2, /* Second order Bezier curve */
	TRAJ_BEZIER3 /* Third order Bezier curve */
};

struct trajectory_line {
	enum trajectory_type id;
	double r0[3], r1[3]; // r0 -> r1 line
};

struct trajectory_bezier2 {
	enum trajectory_type id;
	double P0[3], P1[3], P2[3]; // Second order Bezier curve points
};

struct trajectory_bezier3 {
	enum trajectory_type id;
	double P0[3], P1[3], P2[3], P3[3]; // Second order Bezier curve points
};

union trajectory {
	enum trajectory_type id;
	struct trajectory_line line;
	struct trajectory_bezier2 bezier2;
	struct trajectory_bezier3 bezier3;
};


enum trajectory_calc_task {
	//For given t calculate:
	TRAJECTORY_CALC_R, // R vector
	TRAJECTORY_CALC_DR_DT // First derrivative of R vector by t parameter
};

/**
 * Calculates trajectory values.
 * @param tr - trajectory object  
 * @param t - trajectory parameter \in [0,1]
 * @param v - array of 3 double to put answer to
 * @param task - what to calculate
 */
void trajectory_calc(const union trajectory * tr, double t, double * v, enum trajectory_calc_task task);

/**
 * Some curves need numeric integration to link ther length to t 
 * parameter. For each trajectory is defined a function that creates 
 * t -> length lookup object. Generally, the object needs to be freed
 * after use, despite in some cases this would not be necessary.
 */



enum trajectory_len_type {
	TRAJECTORY_LEN_LINEAR, // 'length' ~linear 't'
	TRAJECTORY_LEN_NUMERIC // length function, caclutated numerically and represented as a cubic spline
};

struct trajectory_len_linear {
	enum trajectory_len_type id;
	double kappa; // l = kappa*t - dependency similar for linear trajectroies
};

struct trajectory_len_numeric {
	enum trajectory_len_type id;
	struct cspline * cs; // A pointer to cspline, stored in heap and ready to calculate values for us
	// The object may be referenced more than once, when freeing a reference counter is envolved
};

union trajectory_len {
	enum trajectory_len_type id;
	struct trajectory_len_linear linear;
	struct trajectory_len_numeric numeric;
};




/**
 * Creates length calculator by given trajectory. The object needs to be
 * destroyed after, because it is possible for calculators to use 
 * @param len - length object pointer to create
 * @param tr - trajectory to create table for
 * @return 0 - OK, 1 - Out of memory, 2 - can't calculate length integral
 */
int trajectory_len_create(union trajectory_len * len, union trajectory * tr);


/**
 * Calculates length from traejctroy beginning by given t.
 * @param len - trajectory length object
 * @param t - \in [0,1]
 * @return length from beginning
 */
double trajectory_len_calc(const union trajectory_len * len, double t);

/**
 * Frees resources, possibly allocated by trajectory_len object
 * @param len - object to free resources of
 */
void trajectory_len_free(union trajectory_len * len);



/**
 * For specified r \in \mathbb{R}^3 point finds t \in [0,1] such that
 * dist(R(t) - r) is maximal. 
 * @param tr - trajectory to study
 * @param r - point calculate maximal distance to, double[3] array
 * @return the distance with appropriate precision (may be a bit more 
 * than actual maximum, since this function is to be used for maximal
 * delay estimation).
 */
double trajectory_max_dist(const union trajectory * tr, const double * r);

/**
 * Copies trajectory length object
 * @param src - object to copy from
 * @param dst - object to copy to
 * @return 0 - OK
 *         1 - out of memory
 */
int trajectory_len_copy(const union trajectory_len * src, union trajectory_len * dst);


/**
 * Creates trajectory reflection in z=0 mirror
 * @param src - trajectory to create reflection for
 * @param refl - the reflection
 */
void trajectory_mirror(const union trajectory * src, union trajectory * refl);

#endif
